function [ u_h condK ] = codinaSolveMatrix_BC_penalty(boundarySamples, boundaryConditions, boundaryShapeFuncVal, boundaryGaussianWeights, boundaryNears, ...
                                                penalty, bc_Type , N, id_bd, xNodes, s_nears, p_s, dp_s, xSamples, w_s )
%CODINASOLVEMATRIX_BC_DIRECT 
%   boundaryGaussianWeights: The gaussian intergation weights over the
%                            boundary.
%   boundaryShapeFuncVal: The Shape functions value for each boundary sample
%                        point.
%   boundaryConditions : The Dirichlet  voundary condition values
%                        (over the sample points)
%   boundaryNears      : The nearest nodes for each boundary sample point.
%   boundarySamples    : The Dirichlet boundary samples
%   penalty  : The penalty parameter.
%   bc_Type  : The type of boundary condition to be used, 0 is homogeneus,
%              1 is standard Codina's problem
%   N        : Number of nodes - not used, keep for compatibility.
%   xNodes   : Node points.
%   xSamples : Sample points.
%   id_bd    : indices for the boundary nodes with dirichlet BC's.
%   w_s      : Gaussian integration weights for each sample point.
%   s_nears  : For each sample point, the nearest nodes.
%   p_s      : Shape function of each node evaluated at each sample point.
%   dp_s     : Shape function Gradient of each node evaluated at each
%              sample point.

% Number of node points
totalNodes      = size(xNodes,1);
% Number of sample points
totalSamples    = size(xSamples,1);

%% Stiffness matrix assembly-----------------------------------------------
%K = sparse( zeros(totalNodes, totalNodes) );
K = zeros(totalNodes, totalNodes);
% For each sample point (Gauss Point)
for k = 1 : totalSamples;
    % Vector with the nearest node points of the k-ith gauss point.
    k_near = s_nears{k};
    % Gradient of the Shape function at the k-ith gauss point.
    dp_k   = dp_s{k};
    % Sum the contribution of the (\nabla u \cdot \nabla v)
    K(k_near,k_near) = K(k_near,k_near) + (dp_k*dp_k') * w_s(k);
end

%% Sums the contribution of the penalty parameter on the system matrix-----
% Construct the penalty contribution matrix M
M = zeros(totalNodes, totalNodes);
totalBoundarySamples = size(boundarySamples,1);    
% For each sample point (Gauss Point) on the Dirichlet Boundary
for k = 1 : totalBoundarySamples;
    % Vector with the nearest node points of the k-ith gauss point.
    k_near = boundaryNears{k};
    % Shape function at the k-ith boundary gauss point.
    p_k   = boundaryShapeFuncVal{k};
    % Sum the contribution of the ( u v)
    for a = 1 : numel(k_near);
        for b = 1 : numel(k_near);
            p_k_a = p_k(a);
            p_k_b = p_k(b);
        % Acumulates the integral value
        M(k_near(a), k_near(b)) = M(k_near(a),k_near(b)) + ...
                        (p_k_a * p_k_b) * boundaryGaussianWeights(k);
        end;
    end;    
end
K = K + penalty *M;

%% Right hand side assembly------------------------------------------------
% Right hand side vector
rhs  = zeros(totalNodes, 1);

%% Homogeneus boundary condition problem
if (bc_Type == 0);
    % for each sample point k
    for k = 1 : totalSamples;
        % Gets the neares Nodes of the k-ith sample point.
        k_near = s_nears{k};
        % number of neares nodes of the k-ith sample point.
        n_k    = length(k_near);
        % Shape function value at the k-ith sample point.
        p_k    = p_s{k};
        % Gaussian integration weight for the k-ith sample point.
        w_gk   = w_s(k);
        % The external "force" (-2y)
        val    = -2.0 * xSamples(k,2) * w_gk;
        
        % loop on the neighbors of the i-th gauss points
        for ia = 1 : n_k;
            i      = k_near(ia);
            rhs(i) = rhs(i) + p_k(ia) * val;
        end;
    end;
end;


%% Sums the contribution of the RHS vector of the penalty terms
rhs_bc = zeros(totalNodes,1);
% For each sample point (Gauss Point) on the Dirichlet Boundary
for k = 1 : totalBoundarySamples;
    % Vector with the nearest node points of the k-ith gauss point.
    k_near = boundaryNears{k};
    % Shape function at the k-ith boundary gauss point.
    p_k   = boundaryShapeFuncVal{k};
    % Sum the contribution
    for a = 1 : numel(k_near);
        p_k_a = p_k(a);
        % Acumulates the integral value
        rhs_bc(k_near(a)) = rhs_bc(k_near(a)) + ...
           p_k_a * boundaryConditions(k)* boundaryGaussianWeights(k);
    end;    
end

rhs = rhs + penalty*rhs_bc;

%% Solve the linear system of equation.------------------------------------
u_h   = K \ rhs;
condK = cond(K);

end

